﻿using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;

namespace RobotsSample
{
    /// <summary>
    /// 小模块机器人算法方法接口
    /// 注 意：在Ubuntu中使用P/Invoke 时发现加不加载 MinibotAlgorithmLib ，将 MinibotAlgorithmLib.so重命令为alib.so,并修改当前引用 MinibotAlgorithmLib 为 alib 可解决。原因未明
    /// </summary>
    public static class MiniBotAlgorithmLib
    {
        ///<summary>
        ///计算关节角度To末端笛卡尔空间
        ///参数： j[5] 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围 I：正负150角度,T：正负90，G：-90到0  （0度的关节模块0位）
        ///输出：笛卡尔空间 X Y Z Roll Pitch Yaw 六个元素的数组
        /// </summary>
        /// <param name="j">j[5]</param>
        /// <returns></returns>
#if X64
        [DllImport("MinibotAlgorithmLib.x64", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#else
        [DllImport("MinibotAlgorithmLib.x86", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#endif
        public static extern IntPtr CalJointAngleToTerminalCartesian(IntPtr j);

        /// <summary>
        ///计算末端笛卡尔空间To关节角度
        ///参数： pose[6] X Y Z Roll Pitch Yaw 六个元素的数组
        ///输出：关节弧度角 float* 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围 I：正负150角度,T：正负90，G：-90到0  （0度的关节模块0位）
        /// </summary>
        /// <param name="p"> byte pose[6]</param>
        /// <returns></returns>
#if X64
        [DllImport("MinibotAlgorithmLib.x64", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#else
        [DllImport("MinibotAlgorithmLib.x86", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#endif
        public static extern IntPtr CalTerminalCartesianToJointAngle(IntPtr p);


        /// <summary>
        /// 添加机器人
        /// </summary>
        /// <param name="robotType">机器人类别：遥控小车、蛇、双足、双轮、攀爬、仿生蜘蛛、X轮式</param>
        /// <param name="ids">
        /// 第一个ID均为与控制底座连接的模块ID，如多个以下分开说明，位置请参考文档说明
        /// 攀爬：T:Down,G:Down,T:Up,G:Up
        /// 双足/双轮：T:Down,T:Down,IU:Down,T:Up,T:Up,IU:Up
        /// </param>
        /// <param name="count"></param>
        /// <returns>返回机器人控制ID</returns>
#if X64
        [DllImport("MinibotAlgorithmLib.x64", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#else
        [DllImport("MinibotAlgorithmLib.x86", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#endif
        public static extern int AddRobot(int robotType, byte[] ids, int count);


        /// <summary>
        /// 移除机器人
        /// </summary>
        /// <param name="id"></param>
        /// <returns></returns>
#if X64
        [DllImport("MinibotAlgorithmLib.x64", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#else
        [DllImport("MinibotAlgorithmLib.x86", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#endif

        public static extern bool RemoveRobot(int id);

        /// <summary>
        /// 对指定机器人发送指令
        /// </summary>
        /// <param name="id">机器人ID</param>
        /// <param name="cmd">命令类别</param>
        /// <param name="parameter">命令参数</param>
        /// <returns></returns>
#if X64
        [DllImport("MinibotAlgorithmLib.x64", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#else
        [DllImport("MinibotAlgorithmLib.x86", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#endif

        public static extern int SendCmd(int id, int cmd, int parameter);


        /// <summary>
        /// 绑定模块控制接口
        /// </summary>
        /// <param name="setPosition"></param>
        /// <param name="setPositionWithSpeed"></param>
        /// <param name="setMultiPosition"></param>
        /// <param name="setMultiPositionWithSpeed"></param>
        /// <param name="setSpeed"></param>
        /// <param name="setRunningSpeed"></param>
        /// <param name="enableTorque"></param>
        /// <param name="setMode"></param>
        /// <param name="threadSleep"></param>
#if X64
        [DllImport("MinibotAlgorithmLib.x64", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#else
        [DllImport("MinibotAlgorithmLib.x86", CallingConvention = CallingConvention.StdCall, ExactSpelling = true)]
#endif

        public static extern bool BindRobotModuleApis(int id, SetPositionCallback setPosition,
            SetPositionWithSpeedCallback setPositionWithSpeed,
            SetMultiPositionCallback setMultiPosition,
            SetMultiPositionWithSpeed setMultiPositionWithSpeed,
            SetSpeedCallback setSpeed,
            SetSpeedCallback setRunningSpeed,
            SetTorqueEnableCallback setEnable,
            SetModeCallback setMode
         );
    }
}
